#include "baseStation.hpp"
#include "canMotor.hpp"
#include "sta_cmd.hpp"

extern cmd_line_group for_ret;

extern "C"
{

int test_basestation()
{
    int a = 0;
    uint8_t buf[64];
    robot_station& t = robot_station::instance();
	rt_thread_delay(5000);
    t.get_data(buf, 64);
    for(; a<64; a++)
    {
        rt_kprintf("---%d---\n", buf[a]);
    }
	return 0;
}

int test_can_motor()
{
    struct rt_can_msg msg = {0};
    struct rt_can_msg msg_r = {0};
    robot_motor& t = robot_motor::instance();

	for(int i=0;i<10;i++)
	{
		t.add_id(i);
		rt_kprintf("test add id %d\n", i);
	}
    
	
	msg.id = 0x01;             
    msg.ide = RT_CAN_STDID;    
    msg.rtr = RT_CAN_DTR;     
    msg.len = 8;   
	
	msg.data[0] = 0x00;
	msg.data[1] = 0x11;
	msg.data[2] = 0x22;
	msg.data[3] = 0x33;
	msg.data[4] = 0x44;
	msg.data[5] = 0x55;
	msg.data[6] = 0x66;
	msg.data[7] = 0x77;

	for(int i=0; i<100; i++)
	{
		t.write(0, &msg, sizeof(msg));

		t.read_block(0, &msg_r, sizeof(msg));
		
		rt_kprintf("\n test count %d \n", i);
		rt_kprintf("[CAN/SED]id: %d %2x %2x\n",msg.id, msg.data[1], msg.data[2]);
		rt_kprintf("[CAN/REC]id: %d %2x %2x\n",msg_r.id, msg_r.data[1], msg_r.data[2]);
		
		msg.data[1]++;
		msg.data[2]++;
		
		msg.id += 1; 
		if(msg.id == 10)
			msg.id = 0;
	}      
}

int test_cmd_type()
{
	for_ret.group[0] = 5;
	for(int i=0; i<4; i++)
	{
		rt_kprintf("%d", for_ret.group[0].cmd[i]);
		rt_kprintf("\n");
	} 
}

}